import cv2
import rclpy
from cv_bridge import CvBridge
from rclpy.node import Node
from sensor_msgs.msg import Image


# ========== 图像发布节点 ==========
class ImagePublisher(Node):
    def __init__(self):
        super().__init__('camera_publisher')
        self.publisher_ = self.create_publisher(Image, 'camera_image', 10)
        self.bridge = CvBridge()

        # 打开默认摄像头（设备编号 0）
        self.cap = cv2.VideoCapture(0)
        if not self.cap.isOpened():
            self.get_logger().error("无法打开摄像头")
            exit(1)

        # 设置定时器（30Hz）
        self.timer = self.create_timer(1.0 / 30.0, self.timer_callback)
        self.get_logger().info('摄像头图像发布节点已启动')

    def timer_callback(self):
        ret, frame = self.cap.read()
        if not ret:
            self.get_logger().warn('未能读取摄像头图像帧')
            return

        # 转为ROS图像消息
        ros_image = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
        ros_image.header.stamp = self.get_clock().now().to_msg()
        self.publisher_.publish(ros_image)

    def destroy_node(self):
        # 释放摄像头资源
        self.cap.release()
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)

    # 启动发布节点
    publisher = ImagePublisher()

    # 启动订阅节点（独立线程）
    executor = rclpy.executors.MultiThreadedExecutor()
    executor.add_node(publisher)

    try:
        executor.spin()  # 多线程执行
    except KeyboardInterrupt:
        pass
    finally:
        publisher.destroy_node()
        rclpy.shutdown()
